import numpy as np
import matplotlib.pyplot as plt
from stepper_motor_motion_profile.motor_math import StepperMotorMath
from stepper_motor_motion_profile.trapezoid_motion import TrapezoidMotion
from stepper_motor_motion_profile.s_motion import S_Motion
from stepper_motor_motion_profile.s_motion_ex import S_Motion_Ex

acc:int = 200
dec:int = 200
start_v:int = 14
max_v:int = 100
end_v:int = 14
step_num = 3200
time:float = 1
spr:int = 1600
timer_freq:int = 1000*1000

def calcu_tn(cn, n)->float:
    tn:float = 0
    if n == 0:
        tn = cn[0]/2
    else:
        for i in range(n):
            tn += cn[i]
        tn = tn + cn[n]/2
    return tn

trapMotion:TrapezoidMotion = TrapezoidMotion()
smotion:S_Motion = S_Motion()
smotion_ex:S_Motion_Ex = S_Motion_Ex()

trapMotion.init(acc, dec, start_v, max_v, end_v, step_num, spr, timer_freq)
smotion.init(start_v, max_v, step_num, timer_freq, spr)
smotion_ex.init(start_v, time, step_num, timer_freq, spr)

# 生成x的取值范围
x = np.arange(0, step_num+10, 1)

t_cn = np.arange(0, step_num+10, 1)
s_cn = np.arange(0, step_num+10, 1)
s_ex_cn = np.arange(0, step_num+10, 1)

for i in range(t_cn.size):
    t_cn[i] = trapMotion.calcu_next_cn()

for i in range(s_cn.size):
    s_cn[i] = smotion.calcu_next_cn()

for i in range(s_ex_cn.size):
    s_ex_cn[i] = smotion_ex.calcu_next_cn()

t_t = [0]*t_cn.size
for i in range(len(t_t)):
    t_t[i] = calcu_tn(t_cn, i)

t_s = [0]*s_cn.size
for i in range(len(t_s)):
    t_s[i] = calcu_tn(s_cn, i)

t_s_ex = [0]*s_ex_cn.size
for i in range(len(t_s_ex)):
    t_s_ex[i] = calcu_tn(s_ex_cn, i)

print(t_t[len(t_t) - 1])
print(t_s[len(t_s) - 1])
print(t_s_ex[len(t_s_ex) - 1])

#梯型加减速的每一步对应的速度
vn = [0.0]*t_cn.size
for i in range(len(vn)):
    vn[i] = StepperMotorMath.cn_2_rpm(t_cn[i], timer_freq, spr)

#S型加减速的每一步对应的速度
svn = [0.0]*s_cn.size
for i in range(len(svn)):
    svn[i] = StepperMotorMath.cn_2_rpm(s_cn[i], timer_freq, spr)

#S_Ex型加减速的每一步对应的速度
svn_ex = [0.0]*s_ex_cn.size
for i in range(len(svn_ex)):
    svn_ex[i] = StepperMotorMath.cn_2_rpm(s_ex_cn[i], timer_freq, spr)

#配置pyplot的字体以显示中文。
plt.rcParams['font.family'] = 'Microsoft YaHei'
plt.scatter(t_t, vn, label = "T型")
#plt.scatter(t_s, svn, label = "S型")
plt.scatter(t_s_ex, svn_ex, label = "S_Ex型")
plt.legend()
plt.show()
